﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using Microsoft.Robotics.Services.DepthCamSensor;
using Microsoft.Robotics.Services.Sensors.Kinect;
namespace NestedParticleFilter
{
    public class DepthAnalysis
    {
        public const int FieldOfView = 57; //degree
        /// <summary>
        /// The distance in mm that the robot identifies as safe to manuver
        /// </summary>
        private const short ClearDistance = 1000; //mm

        /// <summary>
        /// The distance in mm at which an object is an obstacle and requires the robot to stop.
        /// </summary>
        public const short StopDistance = 750;

        /// <summary>
        /// The reciprocal of the distance between the ClearDistance and the StopDistance.
        /// </summary>
        public const double RangeReciprocal = 1.0 / (ClearDistance - StopDistance);


        private const int KernelWidth = 100;


        /// <summary>
        /// outputs the forward and rotational factor for driving the robot based
        /// on depth analysis of the image
        /// </summary>
        /// <param name="depthState">The State of the kinect </param>
        /// <param name="forwardSpeed">forward speed factor between (0,1)</param>
        /// <param name="rotationalSpeed">rotation speed factor between (0,1) </param>
        public static void InterpertDepthImage(DepthCamSensorState depthState, out double forwardSpeed, out double rotationalSpeed)
        {
            short[] summary = GetSmoothDistance(depthState);
            var centerIndex = summary.Length / 2;
            var direction = centerIndex;

            // check the straight-ahead distance. If there is an obstacle approaching, slow down
            // and look for a more open path to the right or left.
            // special case for clear path ahead)
            if (summary[centerIndex] > ClearDistance)
            {
                forwardSpeed = 1.0;
                rotationalSpeed = 0.0;
                return;
            }

            // some obstacles ahead. Adjust speed and rotation
            forwardSpeed = (summary[centerIndex] - StopDistance) * RangeReciprocal;
            forwardSpeed = Math.Max(forwardSpeed, 0);
            for (int offset = 1; offset < centerIndex; offset++)
            {
                if (summary[centerIndex + offset] > ClearDistance)
                {
                    // smooth sailing
                    direction = offset;
                    break;
                }

                if (summary[centerIndex - offset] > ClearDistance)
                {
                    // smooth sailing
                    direction = -offset;
                    break;
                }
            }
            rotationalSpeed = (double)direction / (double)centerIndex;
        }

        /// <summary>
        /// Given the depth state, this method returns the smoothed distance reading of the kinect
        /// </summary>
        /// <param name="depthState"></param>
        /// <returns></returns>
        public static short[] GetSmoothDistance(DepthCamSensorState depthState)
        {
            short[] clear = GetClearDistance(depthState);

            // find the closest distance for each segment of width kernelWidth
            var summary = new short[clear.Length - KernelWidth];
            for (int i = 0; i < summary.Length; i++)
            {
                summary[i] = (short)KinectReservedSampleValues.MaxValidDepth;
                for (int j = i; j < (i + KernelWidth); j++)
                {
                    summary[i] = Math.Min(summary[i], clear[j]);
                }
            }
            return summary;
        }
        
        /// <summary>
        /// Returns the furthest clearest distance in every column of the depth image
        /// </summary>
        /// <param name="depthState"></param>
        /// <returns></returns>
        private static short[] GetClearDistance(DepthCamSensorState depthState)
        {
            int width = depthState.DepthImageSize.Width;
            int height = depthState.DepthImageSize.Height;
            var clear = new short[width]; // maximum depth of each column is saved in this array
            
            // calculate the furthest "clear" distance for each column in the depth image
            for (int column = 0 ; column <width; column++)
            {
                var nearestObstacle = (short) KinectReservedSampleValues.MaxValidDepth;
               
                for (int row = height-1 ; row>0 ; row-- )
                {
                    short depthInfo = GetDepth(depthState.DepthImage, width, column, row);
                    if (depthInfo == (short)KinectReservedSampleValues.PrefilterNoReading)
                        continue;
                    if (depthInfo == (short) KinectReservedSampleValues.Floor)
                        continue;
                    nearestObstacle = Math.Min(depthInfo, nearestObstacle);
                }
                clear[column] = nearestObstacle;
                if (clear[column] >= (short)KinectReservedSampleValues.MaxValidDepth)
                    clear[column] = 0;
            }
            return clear;
        }

        /// <summary>
        /// Get the depth information in a 1D array in mm
        /// </summary>
        /// <param name="depthImage">Depth image</param>
        /// <param name="width">width of the image </param>
        /// <param name="column"> column number </param>
        /// <param name="row"> row number</param>
        /// <returns></returns>
        public static short GetDepth (short [] depthImage, int width, int column, int row)
        {

            return depthImage[(row*width) + column];
        }

        /// <summary>
        /// Given an angle, this function returns the depth information in mm at row 190
        /// </summary>
        /// <param name="angle"></param>
        /// <param name="depthState"></param>
        /// <returns></returns>
        public static short GetDepth(double angle, DepthCamSensorState depthState)
        {
            int width = depthState.DepthImageSize.Width;
            short[] ranges  = new short[width];
            // calculate the furthest "clear" distance for each column in the depth image);
            for (int column = 0; column < width; column++)
            {
                short depthInfo = GetDepth(depthState.DepthImage, width, column, 190);
                ranges[column] = depthInfo;

            }
            int index = (int)(ranges.Length/2 - (angle/(FieldOfView/2.0)));
            //Debug.WriteLine("DepthAnalysis.GetDepth: " + ranges[index]);
            return ranges[index];

        }



        /// <summary>
        /// Get the min range and angle based on observation... IT is used for negative observation
        /// </summary>
        /// <param name="depthState"> depth state of the kinect</param>
        /// <param name="depth">depth in mm</param>
        /// <param name="angle">angle in degree</param>
        public static void GetMinValidDepthAndAngle (DepthCamSensorState depthState, out double depth, out double angle)
        {
            short[] summary = GetSmoothDistance(depthState);
            // find min 
            double min = (double) KinectReservedSampleValues.MaxValidDepth;
            int index = -1;
            double temp = summary[0];
            bool changed = false; // check if all values of summery is the same. if so, angle will be the middle index
            for ( int i = 0 ; i < summary.Count() ; i++)
            {
                if (summary[i] < min && summary[i]!= 0 )
                {
                    min = summary[i];
                    index = i;
                }
                if (summary[i] != temp)
                    changed = true;
            }
            if (index == -1)
            {
                // all values are 0, return middle values
                Debug.WriteLine("Processing kinect vallues... all values are 0");
                depth = 0;
                angle = FieldOfView/2.0;
            }
            else if (!changed) // all values are the same
            {
                depth = min; // mm
                angle = GetAngle(summary.Length, index);
            }
            else
            {
                Debug.WriteLine("\tIndex: " + index);
                depth = min; // mm
                angle = GetAngle(summary.Length, index);
                   
            }
            
        }


        /// <summary>
        /// returns the angle (degree) of a certain pixel relative to the location of the camera based on the
        /// horizontal field of view of the camera
        /// </summary>
        /// <param name="width">width of the image</param>
        /// <param name="column">column of the pixel</param>
        /// <returns>angle of the pixel in degree</returns>
        public static double GetAngle (int width, int column)
        {
            double num = (double)FieldOfView / width; // angles per pixel
            double center = width/2.0;
            double differance = center - column;//column-center; // angle is "difference" pixels to the left
            double angle = differance *num;
            //if (angle < 0)
            //    Debug.WriteLine("To the Right of the robot");
            //else
            //{
            //    Debug.WriteLine("To the Left of the robot");
            //}
            return angle; 
           
        }

       

    }
}
